﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;
using Microsoft.Xna.Framework;

namespace game
{
    public class RegularMovement
    {
        public enum LeanState
        {
            LEFT,
            RIGHT,
            IDLE
        }

        public float LeanStablePointEpsilon { get; set; }

        public float MaxLean { get; set; }
        public float LeanSpeed { get; set; }

        public float MoveAcc { get; set; }

        public float MaxMoveSpeed {
            get { return maxMoveSpeed; }
            set
            {
                maxMoveSpeed = value;
                maxMoveSpeedInv = 1.0f / value;
            }
        }

        private float maxMoveSpeed;
        private float maxMoveSpeedInv;

        public float Lean(float curLean, LeanState dir)
        {
            float rot = curLean;
            if (dir == LeanState.LEFT)
            {
                rot += LeanSpeed * MovementControl.DeltaTimeMs;
                if (rot > MaxLean)
                {
                    rot = MaxLean;
                }
            }
            else if (dir == LeanState.RIGHT)
            {
                rot -= LeanSpeed * MovementControl.DeltaTimeMs;
                if (rot < -MaxLean)
                {
                    rot = -MaxLean;
                }
            }
            else if (dir == LeanState.IDLE)
            {
                if (rot > 0.0f)
                {
                    rot -= LeanSpeed * MovementControl.DeltaTimeMs;
                }
                else if (rot < 0.0f)
                {
                    rot += LeanSpeed * MovementControl.DeltaTimeMs;
                }

                if (Math.Abs(curLean) < LeanStablePointEpsilon)
                {
                    rot = 0.0f;
                }
            }
            return rot;
        }

        public float Rotate(float curRot, float rotSpeed)
        {
            curRot += rotSpeed * MovementControl.DeltaTimeMs;
            while (curRot > MathHelper.Pi)
                curRot -= MathHelper.TwoPi;
            while (curRot <= -MathHelper.Pi)
                curRot += MathHelper.TwoPi;
            return curRot;
        }

        public float timeAccum = 0.0f;

        public void MoveWithVibration(ref Vector3 curPos, float speed, ref Vector3 dir,
            out Vector3 newPosition, out Vector3 velocity)
        {
            Vector3.Multiply(ref dir, speed, out velocity);
            Vector3 movement;
            Vector3.Multiply(ref velocity, MovementControl.DeltaTimeMs, out movement);
            Vector3.Add(ref curPos, ref movement, out newPosition);

            Vector3 up = Vector3.Up;
            Vector3 sideVibration;
            Vector3.Cross(ref up, ref dir, out sideVibration);

            float oldTimeAccum = timeAccum;
            timeAccum += MovementControl.DeltaTimeMs;

            float vibrStrength = speed * maxMoveSpeedInv * 200.0f;
            float sideVibrLength = ((float)Math.Sin(timeAccum * 0.002f)
                - (float)Math.Sin(oldTimeAccum * 0.002f)) * vibrStrength;

            Vector3.Multiply(ref sideVibration, sideVibrLength, out sideVibration);

            Vector3.Add(ref newPosition, ref sideVibration, out newPosition);
        }


        public void MoveWithAdditionalVelocity(ref Vector3 curPos, float speed, ref Vector3 dir,
            ref Vector3 addVelocity, out Vector3 newPosition, out Vector3 velocity)
        {
            Vector3.Multiply(ref dir, speed, out velocity);
            Vector3.Add(ref velocity, ref addVelocity, out velocity);
            Vector3 movement;
            Vector3.Multiply(ref velocity, MovementControl.DeltaTimeMs, out movement);
            Vector3.Add(ref curPos, ref movement, out newPosition);
        }


        // nonnewtonian
        public float ChangeSpeed(float curSpeed, float acc, bool faster)
        {
            if(faster)
            {
                curSpeed += acc * MovementControl.DeltaTimeMs;
                if (curSpeed > maxMoveSpeed)
                    curSpeed = maxMoveSpeed;
            }
            else
            {
                curSpeed -= acc * MovementControl.DeltaTimeMs;
                if (curSpeed < 0.0f)
                    curSpeed = 0.0f;
            }
            return curSpeed;
        }

        // newtonian
        public void EngineOn(ref float curSpeed, ref Vector3 curMoveDir, Vector3 curDir,
            float acc, out float newSpeed, out Vector3 newMoveDir)
        {
            Vector3 velocity, oldVelComponent, newVelComponent;
            Vector3.Multiply(ref curMoveDir, curSpeed, out oldVelComponent);
            Vector3.Multiply(ref curDir, acc * MovementControl.DeltaTimeMs, out newVelComponent);
            Vector3.Add(ref oldVelComponent, ref newVelComponent, out velocity);

            newSpeed = velocity.Length();
            Vector3.Divide(ref velocity, newSpeed, out newMoveDir);
            newSpeed = newSpeed > maxMoveSpeed ? maxMoveSpeed : newSpeed;
        }
    }
}
